import rclpy
import cv2
import cv_bridge
import numpy as np
from rclpy.node import Node
from sensor_msgs.msg import CompressedImage
from origincar_msg.msg import Sign
from geometry_msgs.msg import Twist
from std_msgs.msg import Int32
from ai_msgs.msg import PerceptionTargets
import threading

class Follower(Node):
    def __init__(self):
        super().__init__('line_follower')
        self.get_logger().info("Start line follower.")
        
        self.bridge = cv_bridge.CvBridge()
        
        self.image_sub = self.create_subscription(CompressedImage, '/image/compressed', self.image_callback, 10)
        self.cmd_vel_pub = self.create_publisher(Twist, 'cmd_vel', 10)
        self.qr_pub = self.create_publisher(Sign, '/sign_switch', 10)
        self.pub = self.create_publisher(CompressedImage, '/camera/process_image', 10)
        self.mask_pub = self.create_publisher(CompressedImage, '/camera/mask_image', 10)
        
        self.sign_sub = self.create_subscription(Int32, 'sign_foxglove', self.sign_callback, 10)
        
        self.twist = Twist()
        self.qrDecoder = cv2.QRCodeDetector()  # Create a qrCodeDetector Object
        
        self.is_following_line = True  # 巡线脚本的状态
        self.is_avoiding_obstacle = False  # 避障状态
        self.external_control = False  # 外部控制的标志

        # 添加避障功能的订阅者
        self.obstacle_sub = self.create_subscription(
            PerceptionTargets,
            'hobot_dnn_detection',
            self.obstacle_callback,
            10
        )
        self.lock = threading.Lock()
        self.target_detected = False
        self.obstacle_timer = None  # 定时器
        self.confidence_threshold = 0.50  # 置信度阈值
        self.y_axis_threshold = 300  # 图像框Y轴位置阈值
        self.last_twist_msg = Twist()  # 用于记录最后一次的 Twist 指令
        self.obstacle_center_x = None  # 障碍物中心点X坐标
        self.obstacle_direction = None  # 避障方向

        # 记录避障时的角速度
        self.avoid_angular_z = 0.0

    def sign_callback(self, msg):
        self.get_logger().info(f"Received sign: {msg.data}")
        if msg.data == 5:
            self.is_following_line = False
            self.external_control = True
            self.is_avoiding_obstacle = False
            self.get_logger().info("Deactivating line following due to external control.")
        elif msg.data == 6:
            self.is_following_line = True
            self.external_control = False
            self.get_logger().info("Activating line following.")

    def image_callback(self, msg):
        if not self.is_following_line or self.is_avoiding_obstacle:
            return
        
        self.get_logger().info("Image callback triggered.")
        try:
            np_arr = np.frombuffer(msg.data, np.uint8)
            image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
        except cv_bridge.CvBridgeError as e:
            self.get_logger().error(f"CvBridge Error: {e}")
            return
        
        # Detect and decode the QR code
        data, bbox, rectifiedImage = self.qrDecoder.detectAndDecode(image)
        if data:
            self.get_logger().info(f"QR Code detected: {data}")
            qr_msg = Sign()  # 创建一个Sign消息对象
            self.twist.linear.x = 0.0
            self.twist.angular.z = 0.0
            self.cmd_vel_pub.publish(self.twist)
            if data == 'ClockWise':
                qr_msg.sign_data = 3
            elif data == 'AntiClockWise':
                qr_msg.sign_data = 4
            self.get_logger().info(f'Publishing: {qr_msg.sign_data}')
            self.qr_pub.publish(qr_msg)  # 发布消息到/sign_switch话题
            return
        
        hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
        lower_black = np.array([0, 0, 0])
        upper_black = np.array([180, 255, 50])

        mask = cv2.inRange(hsv, lower_black, upper_black)

        h, w, d = image.shape
        search_top = int(h * 7 / 11)
        search_bot = int(h / 2 + 20)
        mask[0:search_top, 0:w] = 0
        M = cv2.moments(mask)
        
        if M['m00'] > 0:
            cx = int(M['m10'] / M['m00'])
            cy = int(M['m01'] / M['m00'])
            cv2.circle(image, (cx, cy), 20, (0, 0, 255), -1)
            
            # 基于检测的目标中心点，计算机器人的控制参数
            err = cx - w / 2
            self.twist.linear.x = 0.2   # 巡线的速度
            self.twist.angular.z = -float(err) / 575  # 巡线调节的角度P调整
            if not self.external_control:
                self.cmd_vel_pub.publish(self.twist)

        try:
            compressed_image = CompressedImage()
            compressed_image.header.stamp = self.get_clock().now().to_msg()
            compressed_image.format = "jpeg"
            compressed_image.data = np.array(cv2.imencode('.jpg', image)[1]).tobytes()
            self.pub.publish(compressed_image)

            mask_image = CompressedImage()
            mask_image.header.stamp = self.get_clock().now().to_msg()
            mask_image.format = "jpeg"
            mask_image.data = np.array(cv2.imencode('.jpg', mask)[1]).tobytes()
            self.mask_pub.publish(mask_image)
        except cv_bridge.CvBridgeError as e:
            self.get_logger().error(f"Publish CvBridge Error: {e}")

    def obstacle_callback(self, msg):
        with self.lock:
            if self.is_avoiding_obstacle:
                return

            obstacle_detected = False
            for target in msg.targets:
                confidence = target.rois[0].confidence
                y_offset = target.rois[0].rect.y_offset
                height = target.rois[0].rect.height

                self.get_logger().info(f"检测到目标: 置信度={confidence}, y轴底部位置={y_offset + height}")
                if confidence >= self.confidence_threshold and y_offset + height >= self.y_axis_threshold:
                    self.get_logger().info(f"符合条件的障碍物: 置信度={confidence}, y轴底部位置={y_offset + height}")
                    if target.rois[0].type == "hindrance":
                        self.get_logger().info("调用避障逻辑")
                        self.avoid_obstacle(target)
                        self.target_detected = True
                        self.is_avoiding_obstacle = True  # 停止进一步的障碍物检测
                        obstacle_detected = True
                        # 确保只有一个定时器
                        if self.obstacle_timer:
                            self.obstacle_timer.cancel()
                        # 恢复障碍物检测的定时器创建， 3.4s后自动调用回调函数
                        self.obstacle_timer = threading.Timer(3.4, self.reset_target_detected)  
                        self.obstacle_timer.start()        # 启动定时器
                        return

            if not msg.targets or not obstacle_detected:
                self.get_logger().info("未检测到目标，设置定时器恢复巡线")
                self.obstacle_timer = threading.Timer(0.5, self.reset_target_detected)
                self.obstacle_timer.start()

    def avoid_obstacle(self, target):
        self.get_logger().info("执行避障逻辑")
        self.obstacle_center_x = target.rois[0].rect.x_offset + target.rois[0].rect.width / 2
        self.get_logger().info(f"障碍物中心点={self.obstacle_center_x}")
        if self.obstacle_center_x < 320 - 50:  # 障碍物在左边
            self.obstacle_direction = 'right'
            self.turn_angle('right', 1.1, 0.85)  # 调整时间
            self.get_logger().info("向右避开障碍物")
        elif self.obstacle_center_x > 320 + 50:  # 障碍物在右边
            self.obstacle_direction = 'left'
            self.turn_angle('left', 1.1, 1.0)  # 调整时间
            self.get_logger().info("向左避开障碍物")
        else:  # 障碍物在中心，根据偏左还是偏右决定转向方向
            if self.obstacle_center_x < 320 - 20:
                self.obstacle_direction = 'right'
                self.get_logger().info("向右避开障碍物")
                self.turn_angle('right', 2.2, 0.9)  # 调整时间
            else:
                self.obstacle_direction = 'left'
                self.get_logger().info("向左避开障碍物")
                self.turn_angle('left', 2.2, 1.2)  # 调整时间

    def turn_angle(self, direction, ratio, turn_time):
        twist_msg = Twist()
        twist_msg.linear.x = 0.2  # 设置转弯时的线速度为0.2
        self.avoid_angular_z = ratio if direction == 'left' else -ratio  # 记录当前的角速度
        twist_msg.angular.z = self.avoid_angular_z
        self.cmd_vel_pub.publish(twist_msg)
        
        # 启动定时器控制转弯时间
        threading.Timer(turn_time, self.drive_straight).start()

    def drive_straight(self):
        twist_msg = Twist()
        twist_msg.linear.x = 0.2
        twist_msg.angular.z = 0.0  # 确保直行时没有角速度
        self.cmd_vel_pub.publish(twist_msg)
        self.get_logger().info("正在转后直行")
        
        # 直行后恢复原方向
        threading.Timer(0.45, self.resume_direction).start()

    def resume_direction(self):
        twist_msg = Twist()
        twist_msg.linear.x = 0.2
        resume_time = 0
        
        # 恢复方向，使用之前避障时的角速度的反向
        if self.avoid_angular_z <= 0:
            # 避障往右，回来往左
            twist_msg.angular.z = -self.avoid_angular_z
            resume_time = 1.0
        elif self.avoid_angular_z > 0:
            # 避障往左，回来往右
            twist_msg.angular.z = -0.7 * self.avoid_angular_z
            resume_time = 0.36
    
        self.cmd_vel_pub.publish(twist_msg)
        self.get_logger().info(f"逆转恢复方向, 角速度: {twist_msg.angular.z}, 恢复时间: {resume_time}")
    
        # 启动定时器控制恢复巡线
        threading.Timer(resume_time, self.final_straight).start()


    def final_straight(self):
        twist_msg = Twist()
        twist_msg.linear.x = 0.2
        twist_msg.angular.z = 0.0  # 最终直行
        self.cmd_vel_pub.publish(twist_msg)
        self.get_logger().info("逆转后直线行驶")
        threading.Timer(0.25, self.resume_line_following).start()

    def resume_line_following(self):
        self.get_logger().info("恢复巡线操作")
        self.is_following_line = True  # 恢复巡线状态
        self.is_avoiding_obstacle = False  # 确保障碍物检测状态也恢复
        # 重新开始巡线，不使用之前的速度指令
        self.twist.linear.x = 0.2
        self.twist.angular.z = 0.0
        self.cmd_vel_pub.publish(self.twist)
        self.get_logger().info("恢复巡线状态设置完成")

    def reset_target_detected(self):
        with self.lock:
            self.target_detected = False
            self.obstacle_center_x = None
            self.is_avoiding_obstacle = False  # 恢复障碍物检测
        self.get_logger().info("重置 target_detected，恢复巡线")
        self.resume_line_following()

def main(args=None):
    rclpy.init(args=args)
    follower = Follower()
    rclpy.spin(follower)
    follower.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
